#ifndef OOPSMP_CORE_STATE_SPACE_PLUGIN_ARTICULATED_STATE_SPACE_H_
#define OOPSMP_CORE_STATE_SPACE_PLUGIN_ARTICULATED_STATE_SPACE_H_

#include "OOPSMP/Utils/Geometry/Workspace/ArticulatedPart.h"
#include "OOPSMP/Core/StateSpace/StateSpace.H"
#include "OOPSMP/Utils/Math/Topology/SE2Topology.H"
#include "OOPSMP/Utils/Math/Topology/SE3Topology.H"

#include "OOPSMP/Utils/Math/Algebra/vector3.h"
#include "OOPSMP/Utils/Math/Algebra/matrix44.h"

namespace OOPSMP {
	
	/**
	 *@brief Macro for class, class pointer, and const class pointer
	 *       declarations 
	 */
	ForwardClassDeclaration(ArticulatedStateSpace);

	/**
	 *@brief articulated state space
	 *
	 */   
	class ArticulatedStateSpace : public StateSpace {

	public:

	    /**
		 * @brief Set state dimension of articulated state space
		 * @param dim number of degrees of freedom
		 */	
		void setStateDimension( const int dim ) {
			m_dimState = dim;
		}

		/**
		 * @brief Set basis state dimension of articulated state space
		 * @param dim number of dimensions (2 or 3)
		 */
		void setBasisStateDimension( const int dim ) {
			m_dimBasisState = dim;
		}

		/**
		 * @brief Get basis state dimension of articulated state space
		 * @returns number of dimensions (2 or 3)
		 */
		const int getBasisStateDimenstion() const {
			return m_dimBasisState;
		}

		/**
		 *@brief Apply transformation implied by the state <em>x</em>
		 *       to the corresponding articulated robot 
		 *
		 *@param x state
		 */
		void applyState(State_const_t x) const
		{   
			getCoreRobotData()->getCollisionDetector()->transformMovingPart(getCoreRobotData()->getMovingPartId(), x);
		}

		/**
		 *@brief Sample values for the state inside the state bounding
		 *       box and respect the topology of the state space 
		 *
		 *@param x state
		 */	
		void sampleUniformState(State_t x) const
		{

			ArticulatedPart_t ap = (ArticulatedPart_t)getCoreRobotData()->getWorkspace()->getMovingPart(getCoreRobotData()->getMovingPartId());

			if (m_dimBasisState == 3)
				SE3Topology::getSingleton()->groupUniformRandom(x, getStateBoundingBox()); // fills first 7 elements of x
			else
				SE2Topology::getSingleton()->groupUniformRandom(x, getStateBoundingBox());

			long num_links = ap->getLinkCount(); // number of links in the completed snake
			long curr_index = (m_dimBasisState == 3) ? 7 : 3; // current index in configurations vector

			for (long i = 1; i < num_links; i++) // tail link is already configured - skip it
			{
				int num_DOFs = ap->getNumDoFsForLink(i);

				double * min = new double[num_DOFs];
				double * max = new double[num_DOFs];

				ap->getLinkJoint(i)->getJointLimits( min, max );

				// sample each DoF for the current link
				for (int j = 0; j < num_DOFs; j++)
				{
					double random = OOPSMPminmax_uniform_random(min[j], max[j]);
					x[curr_index++] = random;
				}

				// clean up
				delete [] max;
				delete [] min;
			}
		}

		void sampleGaussianState(State_const_t x, const double stddev, State_t gx) const { }

		/**
		 *@brief Return true iff the state <em>x</em> satisfies the state invariant
		 *
		 *@param x state
		 */
		bool isStateInvariantSatisfied(State_const_t x) const
		{
			ArticulatedPart_t ap = (ArticulatedPart_t)getCoreRobotData()->getWorkspace()->getMovingPart(getCoreRobotData()->getMovingPartId());

			long n = ap->getLinkCount();

			for (int i = 0; i < n; i++)
			{

				OOPSMPdeclare_point3(point);
				ap->getSomePointForLink(i, point);

				double elt[7] = {point[0], point[1], point[2], 0, 0, 0, 0};

				if (!(SE3Topology::getSingleton()->groupInsideBBox(elt, getStateBoundingBox()))) // getStateBoundingBox() == world limits
					return false;
			}

			return true;
			// return true;
		}

		/**
		 *@brief Return true iff the state <em>x</em> satisfies the
		 *       state invariant and has neither inner nor external collisions 
		 *
		 *@param x state
		 */
		virtual bool isStateValid(State_const_t x) const
		{
			applyState(x);

			ArticulatedPart_const_t ap = dynamic_cast<ArticulatedPart_const_t>(getCoreRobotData()->getWorkspace()->getMovingPart(getCoreRobotData()->getMovingPartId()));
			
			bool hasNoInternalCollisions = ap->isCollisionFree( ap->getRoodId() );
			bool isStateInvSat = isStateInvariantSatisfied(x);
			bool hasNoExternalCollisions = isStateCollisionFree(x);
			
			return isStateInvSat && hasNoInternalCollisions &&  hasNoExternalCollisions;
		}

		/**
		 *@brief Return true iff states <em>x1</em> and <em>x2</em> are the same states
		 *
		 *@param x1 state
		 *@param x2 state
		 */
		bool sameStates(State_const_t x1, State_const_t x2) const
		{
			for(int i = 0; i < m_dimState; i++)
				if(fabs(x1[i] - x2[i]) > OOPSMP_SQRT_EPSILON)
					return false;
			return true;
		}

		/**
		 *@brief Return true iff the gap between <em>xfrom</em> and <em>xto</em> is acceptable for motion planning purposes
		 *
		 *@param xfrom state
		 *@param xto  state
		 */
		bool acceptableGap(State_const_t xfrom, State_const_t xto) const
		{
			return sameStates(xfrom, xto);
		}

	protected:

		/**
		 *@brief determines whether we work in 2D or 3D
		 *
		 */
		int m_dimBasisState;

	};

	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */  
	DeclareNoInstanceFactory(ArticulatedStateSpace, StateSpaceFactory);

}

#endif
